/*
 * File: Fu_RotAngObsPLL.c
 *
 * Code generated for Simulink model 'Fu_RotAngObsPLL'.
 *
 * Model version                  : 1.691
 * Simulink Coder version         : 8.2 (R2012a) 29-Dec-2011
 * TLC version                    : 8.2 (Dec 29 2011)
 * C/C++ source code generated on : Sun Jun 09 21:12:43 2013
 *
 * Target selection: ert.tlc
 * Embedded hardware selection: ARM Compatible->ARM Cortex
 * Code generation objectives: Unspecified
 * Validation result: Not run
 */

#include "Fu_RotAngObsPLL.h"
#include "Fu_RotAngObsPLL_private.h"

/* Exported block signals */
int32_T loc_PLL_Ud_Diff;               /* '<S4>/Add' */
int16_T RotAngObsPLL_RotSpdRPM;        /* '<S1>/Product2' */
int16_T loc_PLL_MathSpd;               /* '<S3>/Product3' */
uint16_T RotAngObsPLL_RotPos;          /* '<S2>/Data Type Conversion' */
int16_T RotAngObsPLL_RotSpRadSec;      /* '<S1>/Data Type Conversion' */

/* Block signals (auto storage) */
BlockIO_Fu_RotAngObsPLL Fu_RotAngObsPLL_B;

/* Block states (auto storage) */
D_Work_Fu_RotAngObsPLL Fu_RotAngObsPLL_DWork;

/* Real-time model */
RT_MODEL_Fu_RotAngObsPLL Fu_RotAngObsPLL_M_;
RT_MODEL_Fu_RotAngObsPLL *const Fu_RotAngObsPLL_M = &Fu_RotAngObsPLL_M_;

/* Model step function */
void Fu_RotAngObsPLL_step(void)
{
  int32_T cff;
  int16_T acc;
  int32_T j;
  int16_T rtb_DiscreteTimeIntegrator;
  int32_T tmp;

  /* Sum: '<S5>/Subtract' incorporates:
   *  Inport: '<Root>/In2'
   *  Memory: '<S5>/Memory'
   */
  tmp = Foc_CurrQ * 1000;
  if (tmp > 32767) {
    tmp = 32767;
  } else {
    if (tmp < -32768) {
      tmp = -32768;
    }
  }

  cff = Fu_RotAngObsPLL_DWork.Memory_PreviousInput_g * 1000;
  if (cff > 32767) {
    cff = 32767;
  } else {
    if (cff < -32768) {
      cff = -32768;
    }
  }

  tmp -= cff;
  if (tmp > 32767) {
    tmp = 32767;
  } else {
    if (tmp < -32768) {
      tmp = -32768;
    }
  }

  /* Product: '<S5>/Divide' incorporates:
   *  Sum: '<S5>/Subtract'
   */
  tmp = tmp * 8791 >> 10;
  if (tmp > 32767) {
    tmp = 32767;
  } else {
    if (tmp < -32768) {
      tmp = -32768;
    }
  }

  tmp = tmp * 16777 >> 14;
  if (tmp > 32767) {
    tmp = 32767;
  } else {
    if (tmp < -32768) {
      tmp = -32768;
    }
  }

  rtb_DiscreteTimeIntegrator = (int16_T)tmp;

  /* Saturate: '<S5>/Saturation' incorporates:
   *  Product: '<S5>/Divide'
   */
  if ((int16_T)tmp >= 10000) {
    rtb_DiscreteTimeIntegrator = 10000;
  } else {
    if ((int16_T)tmp <= -10000) {
      rtb_DiscreteTimeIntegrator = -10000;
    }
  }

  /* End of Saturate: '<S5>/Saturation' */

  /* Product: '<S3>/Product' incorporates:
   *  Inport: '<Root>/In2'
   */
  tmp = 125 * Foc_CurrQ;
  tmp = (((tmp < 0 ? 127 : 0) + tmp) >> 7) * 4295;

  /* Product: '<S3>/Product1' */
  cff = rtb_DiscreteTimeIntegrator * 5;
  cff = (((cff < 0 ? 131071 : 0) + cff) >> 17) * 10995;

  /* Product: '<S3>/Product2' incorporates:
   *  Inport: '<Root>/In1'
   */
  j = 5 * Foc_CurrD;
  j = (((j < 0 ? 131071 : 0) + j) >> 17) * 10995;

  /* Product: '<S3>/Product3' incorporates:
   *  Inport: '<Root>/In4'
   *  Product: '<S3>/Product'
   *  Product: '<S3>/Product1'
   *  Product: '<S3>/Product2'
   *  Sum: '<S3>/Add'
   *  Sum: '<S3>/Add1'
   */
  loc_PLL_MathSpd = div_s16s32((int16_T)((int16_T)((int16_T)(Foc_VoltReqQ * 5243
    >> 11) - ((int16_T)(((tmp < 0 ? 4095 : 0) + tmp) >> 12) >> 1)) - (((cff < 0 ?
    8191 : 0) + cff) >> 14)) << 10, ((((j < 0 ? 8191 : 0) + j) >> 13) << 6) + 88);

  /* Sum: '<S6>/Subtract' incorporates:
   *  Inport: '<Root>/In1'
   *  Memory: '<S6>/Memory'
   */
  tmp = Foc_CurrD * 1000;
  if (tmp > 32767) {
    tmp = 32767;
  } else {
    if (tmp < -32768) {
      tmp = -32768;
    }
  }

  cff = Fu_RotAngObsPLL_DWork.Memory_PreviousInput_b * 1000;
  if (cff > 32767) {
    cff = 32767;
  } else {
    if (cff < -32768) {
      cff = -32768;
    }
  }

  tmp -= cff;
  if (tmp > 32767) {
    tmp = 32767;
  } else {
    if (tmp < -32768) {
      tmp = -32768;
    }
  }

  /* Product: '<S6>/Divide' incorporates:
   *  Sum: '<S6>/Subtract'
   */
  tmp = tmp * 8791 >> 10;
  if (tmp > 32767) {
    tmp = 32767;
  } else {
    if (tmp < -32768) {
      tmp = -32768;
    }
  }

  tmp = tmp * 16777 >> 14;
  if (tmp > 32767) {
    tmp = 32767;
  } else {
    if (tmp < -32768) {
      tmp = -32768;
    }
  }

  rtb_DiscreteTimeIntegrator = (int16_T)tmp;

  /* Saturate: '<S6>/Saturation' incorporates:
   *  Product: '<S6>/Divide'
   */
  if ((int16_T)tmp >= 10000) {
    rtb_DiscreteTimeIntegrator = 10000;
  } else {
    if ((int16_T)tmp <= -10000) {
      rtb_DiscreteTimeIntegrator = -10000;
    }
  }

  /* End of Saturate: '<S6>/Saturation' */

  /* Product: '<S4>/Product2' */
  tmp = rtb_DiscreteTimeIntegrator * 5;
  tmp = (((tmp < 0 ? 131071 : 0) + tmp) >> 17) * 10995;

  /* Product: '<S4>/Product' incorporates:
   *  Inport: '<Root>/In1'
   */
  cff = 125 * Foc_CurrD;
  cff = (((cff < 0 ? 127 : 0) + cff) >> 7) * 4295;

  /* Sum: '<S4>/Add' incorporates:
   *  Inport: '<Root>/In3'
   *  Product: '<S4>/Product'
   *  Product: '<S4>/Product2'
   */
  j = Foc_VoltReqD * 5243 >> 11;
  if (j > 32767) {
    j = 32767;
  } else {
    if (j < -32768) {
      j = -32768;
    }
  }

  cff = ((int16_T)(((cff < 0 ? 4095 : 0) + cff) >> 12) >> 1) - j;
  if (cff > 32767) {
    cff = 32767;
  } else {
    if (cff < -32768) {
      cff = -32768;
    }
  }

  tmp = ((int16_T)(((tmp < 0 ? 8191 : 0) + tmp) >> 13) >> 1) + cff;
  if (tmp > 32767) {
    tmp = 32767;
  } else {
    if (tmp < -32768) {
      tmp = -32768;
    }
  }

  /* Product: '<S4>/Product1' incorporates:
   *  Inport: '<Root>/In2'
   *  Memory: '<S1>/Memory'
   */
  cff = Foc_CurrQ * 5;
  cff = (((cff < 0 ? 131071 : 0) + cff) >> 17) *
    Fu_RotAngObsPLL_B.RotAngObsPLL_RotSpdRaw;
  cff = (((cff < 0 ? 7 : 0) + cff) >> 3) * 10995;

  /* Sum: '<S4>/Add' incorporates:
   *  Product: '<S4>/Product1'
   */
  tmp -= ((cff < 0 ? 8191 : 0) + cff) >> 14;
  if (tmp > 32767) {
    tmp = 32767;
  } else {
    if (tmp < -32768) {
      tmp = -32768;
    }
  }

  loc_PLL_Ud_Diff = tmp * 25 >> 1;

  /* Product: '<S1>/Product3' */
  tmp = (int16_T)mul_s32_s32_s32_sr3_zero(loc_PLL_Ud_Diff, 5) * 5243;
  rtb_DiscreteTimeIntegrator = (int16_T)(((tmp < 0 ? 4095 : 0) + tmp) >> 12);

  /* Sum: '<S1>/Add1' incorporates:
   *  DiscreteIntegrator: '<S1>/Discrete-Time Integrator'
   *  Product: '<S1>/Product1'
   */
  tmp = (Fu_RotAngObsPLL_DWork.DiscreteTimeIntegrator_DSTATE >> 4) +
    loc_PLL_MathSpd;
  if (tmp > 32767) {
    tmp = 32767;
  } else {
    if (tmp < -32768) {
      tmp = -32768;
    }
  }

  tmp += rtb_DiscreteTimeIntegrator >> 5;
  if (tmp > 32767) {
    tmp = 32767;
  } else {
    if (tmp < -32768) {
      tmp = -32768;
    }
  }

  Fu_RotAngObsPLL_B.RotAngObsPLL_RotSpdRaw = (int16_T)tmp;

  /* End of Sum: '<S1>/Add1' */

  /* DiscreteFir: '<S1>/LowPass' */
  acc = (int16_T)(Fu_RotAngObsPLL_B.RotAngObsPLL_RotSpdRaw * 3277 >> 15);
  cff = 1;
  for (j = Fu_RotAngObsPLL_DWork.LowPass_circBuf; j < 2; j++) {
    acc += (int16_T)(Fu_RotAngObsPLL_DWork.LowPass_states[j] *
                     Fu_RotAngObsPLL_ConstP.LowPass_Coefficients[cff] >> 14) >>
      1;
    cff++;
  }

  for (j = 0; j < Fu_RotAngObsPLL_DWork.LowPass_circBuf; j++) {
    acc += (int16_T)(Fu_RotAngObsPLL_DWork.LowPass_states[j] *
                     Fu_RotAngObsPLL_ConstP.LowPass_Coefficients[cff] >> 14) >>
      1;
    cff++;
  }

  /* DataTypeConversion: '<S1>/Data Type Conversion' incorporates:
   *  DiscreteFir: '<S1>/LowPass'
   */
  RotAngObsPLL_RotSpRadSec = (int16_T)(acc * 5 >> 2);

  /* Sum: '<S2>/Add' incorporates:
   *  DiscreteIntegrator: '<S1>/Discrete-Time Integrator'
   *  Memory: '<S2>/Memory'
   *  Product: '<S2>/Product'
   *  Sum: '<S1>/Add'
   */
  cff = (int16_T)((int16_T)((Fu_RotAngObsPLL_DWork.DiscreteTimeIntegrator_DSTATE
    >> 4) + loc_PLL_MathSpd) + (rtb_DiscreteTimeIntegrator >> 5)) * 11 * 125 +
    Fu_RotAngObsPLL_DWork.Memory_PreviousInput;

  /* Switch: '<S2>/Switch2' incorporates:
   *  Constant: '<S2>/Kv_Inv7'
   *  RelationalOperator: '<S2>/Relational Operator2'
   *  Sum: '<S2>/Add1'
   */
  if (cff < 0) {
    cff += 628318531;
  }

  /* End of Switch: '<S2>/Switch2' */

  /* Switch: '<S2>/Switch3' incorporates:
   *  Constant: '<S2>/Kv_Inv9'
   *  RelationalOperator: '<S2>/Relational Operator3'
   *  Sum: '<S2>/Add2'
   */
  if (cff >= 628318531) {
    cff -= 628318531;
  }

  /* End of Switch: '<S2>/Switch3' */

  /* DataTypeConversion: '<S2>/Data Type Conversion' */
  RotAngObsPLL_RotPos = (uint16_T)mul_s32_s32_s32_sr42(cff, 439804651);

  /* Product: '<S1>/Product2' incorporates:
   *  DiscreteFir: '<S1>/LowPass'
   */
  tmp = acc * 5;
  tmp = ((tmp < 0 ? 3 : 0) + tmp) >> 2;
  if (tmp > 32767) {
    tmp = 32767;
  } else {
    if (tmp < -32768) {
      tmp = -32768;
    }
  }

  RotAngObsPLL_RotSpdRPM = (int16_T)tmp;

  /* End of Product: '<S1>/Product2' */

  /* Update for Memory: '<S5>/Memory' incorporates:
   *  Inport: '<Root>/In2'
   */
  Fu_RotAngObsPLL_DWork.Memory_PreviousInput_g = Foc_CurrQ;

  /* Update for DiscreteIntegrator: '<S1>/Discrete-Time Integrator' */
  if (Fu_RotAngObsPLL_DWork.DiscreteTimeIntegrator_DSTATE >= 25600) {
    Fu_RotAngObsPLL_DWork.DiscreteTimeIntegrator_DSTATE = 25600;
  } else {
    if (Fu_RotAngObsPLL_DWork.DiscreteTimeIntegrator_DSTATE <= -25600) {
      Fu_RotAngObsPLL_DWork.DiscreteTimeIntegrator_DSTATE = -25600;
    }
  }

  /* End of Update for DiscreteIntegrator: '<S1>/Discrete-Time Integrator' */

  /* Update for Memory: '<S6>/Memory' incorporates:
   *  Inport: '<Root>/In1'
   */
  Fu_RotAngObsPLL_DWork.Memory_PreviousInput_b = Foc_CurrD;

  /* Update for DiscreteFir: '<S1>/LowPass' */
  Fu_RotAngObsPLL_DWork.LowPass_circBuf--;
  if (Fu_RotAngObsPLL_DWork.LowPass_circBuf < 0) {
    Fu_RotAngObsPLL_DWork.LowPass_circBuf = 1;
  }

  Fu_RotAngObsPLL_DWork.LowPass_states[Fu_RotAngObsPLL_DWork.LowPass_circBuf] =
    Fu_RotAngObsPLL_B.RotAngObsPLL_RotSpdRaw;

  /* End of Update for DiscreteFir: '<S1>/LowPass' */

  /* Update for Memory: '<S2>/Memory' */
  Fu_RotAngObsPLL_DWork.Memory_PreviousInput = cff;
}

/* Model initialize function */
void Fu_RotAngObsPLL_initialize(void)
{
  /* Registration code */

  /* initialize error status */
  rtmSetErrorStatus(Fu_RotAngObsPLL_M, (NULL));

  /* block I/O */
  (void) memset(((void *) &Fu_RotAngObsPLL_B), 0,
                sizeof(BlockIO_Fu_RotAngObsPLL));

  /* exported global signals */
  loc_PLL_Ud_Diff = 0;
  RotAngObsPLL_RotSpdRPM = 0;
  loc_PLL_MathSpd = 0;
  RotAngObsPLL_RotPos = 0U;
  RotAngObsPLL_RotSpRadSec = 0;

  /* states (dwork) */
  (void) memset((void *)&Fu_RotAngObsPLL_DWork, 0,
                sizeof(D_Work_Fu_RotAngObsPLL));

  /* InitializeConditions for DiscreteFir: '<S1>/LowPass' */
  Fu_RotAngObsPLL_DWork.LowPass_circBuf = 0;
  Fu_RotAngObsPLL_DWork.LowPass_states[0] = 0;
  Fu_RotAngObsPLL_DWork.LowPass_states[1] = 0;
}

/*
 * File trailer for generated code.
 *
 * [EOF]
 */
